﻿using System;
using System.IO;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;
using MotionPlanning.MotionPlanningPrimitives;
using MotionPlanning.MotionPlanningComponents;
using MotionPlanning.MotionPlanningComponents.Visualizations;
using MotionPlanning.MotionPlanningComponents.MotionPlanningSystems;

namespace MotionPlanning.MotionPlanningAlgorithms.ioRRT_Prototype
{
    class ioRRT : Planner_RRTBased
    {
        //--------------------------------------------
        protected PointSetComponent V_elite;
        protected PointSetComponent V_elder;

        private double delta_compete;
        private double delta_near;

        private int check_point_0;
        private int check_point_1;
        private int check_point_2;
        private int check_point_3;
        private int check_point_4;
        private int check_point_5;

        public ioRRT(RoboticSystemComponent system, VisualizationComponent visualizer, string motionPlanningConfigFile)
            : base(system, visualizer, motionPlanningConfigFile)
        {
            V_elite = this.Vertices;
            V_elder = new PointSetComponent(
                this.System.Get_DOF_state(),
                this.System.Get_q_max(),
                this.System.Get_q_min(),
                this.System.Get_q_scale(),
                this.System.Get_q_topology(),
                15,
                false);

            TextReader tr = new StreamReader(motionPlanningConfigFile);
            if (tr == null)
                throw new Exception("input file cannot be opened.");
            string rawFile = tr.ReadToEnd();

            string[] separators = new string[] { "<R_p>", "</R_p>" };
            string[] entries = rawFile.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return;
            this.delta_compete = MotionPlanningUtils.StringToDoubleArray(entries[1])[0];

            separators = new string[] { "<R_near>", "</R_near>" };
            entries = rawFile.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return;
            this.delta_near = MotionPlanningUtils.StringToDoubleArray(entries[1])[0];

            separators = new string[] { "<goal_check>", "</goal_check>" };
            entries = rawFile.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return;
            this.doGoalCheck = MotionPlanningUtils.StringToBool(entries[1]);

            tr.Close();

            check_point_0 = 0;
            check_point_1 = 0;
            check_point_2 = 0;
            check_point_3 = 0;
            check_point_4 = 0;
            check_point_5 = 0;
        }

        /// <summary>
        /// Execute ioRRT algorithm
        /// </summary>
        /// <param name="n_iterations"></param>
        public override void Execute(int n_iterations)
        {
            RK4Helper pad = new RK4Helper(System.Get_DOF_state());
            if (x_root == null)
                throw new Exception("x_root uninitialized. Forget to call SetRoot()?");
            int iterations_thisRound = 0;
            while (iterations_thisRound < n_iterations)
            {
                ++iterations_total;
                ++iterations_thisRound;

                check_point_0++;

                #region SharedVariables
                Node_ioRRT x_nearest = null;
                Node_ioRRT x_new = null;
                #endregion

                /*
                    ----------------- Sampling Phase -------------------
                    */
                double[] q_sample = this.Sampler.NextQ(SamplingComponent.SamplingMode.SAMPLING, this.System);
                /*
                 ---------- Nearest Neighbor Search Phase -----------
                 */


                check_point_1++; //---------------------------------------------
                /*
                 ----------------- BestNear Phase ------------------
                 */
                Node_ioRRT x_min = null;
                bool doBestNear = true;
                if (doBestNear == true)
                {
                    int K = (int)(1 + Math.Log(this.Vertices.GetSize(), Math.E));
                    List<object> X_kNearest = null;
                    //this.Vertices.NearestNeighbors_KClosest_Exclusive(q_sample, K, ref X_kNearest);
                    this.Vertices.NearestNeighbors_WithinRange_Exclusive(q_sample, delta_near, ref X_kNearest);
                    double c_min = double.MaxValue;
                    
                    for (int i = 0; i < X_kNearest.Count; ++i)
                    {
                        Node_ioRRT x_cur = (Node_ioRRT)X_kNearest[i];
                        double c_cur = x_cur.get_Value().get_utility_c();
                        if (c_cur < c_min)
                        {
                            c_min = c_cur;
                            x_min = x_cur;
                        }
                    }
                }
                x_nearest = x_min;
                if (x_nearest == null)
                {
                    double dist_nearest = double.MaxValue;
                    Object obj_nearest = null;
                    this.V_elite.NearestNeighbor_Approximate(q_sample, ref dist_nearest, ref obj_nearest);
                    if (obj_nearest == null) continue;
                    x_nearest = (Node_ioRRT)obj_nearest;
                }

                //propagation
                double[] u_inc = this.Sampler.NextControl(SamplingComponent.SamplingMode.CONTROL, this.System);
                double t_inc = u_inc[u_inc.Length - 1];
                double[] q_new = new double[this.System.Get_DOF_state()];
                double cost_inc = 0;
                bool succ_propagation = this.System.Propagate(pad, x_nearest.get_Value().get_q(), u_inc, t_inc, ref q_new, ref cost_inc);
                if (succ_propagation == false) continue;

                bool inRange = this.System.PointInRange(q_new);
                if (inRange == false) continue;

                check_point_3++; //-------------------------------------

                double[] q_1 = x_nearest.get_Value().get_q();
                double[] q_2 = q_new;
                double dx = q_2[0] - q_1[0];
                double dy = q_2[1] - q_1[1];
                double dxy = Math.Sqrt(dx * dx + dy * dy);

                if (dxy > 30)
                    Debugger.Break();


                //try to grow tree
                x_new = (Node_ioRRT)this.SpawnNode_As(q_new);
                Edge e_new = this.SpawnEdge(x_nearest, x_new);
                e_new.get_Value().set_ctrl_inc(u_inc);
                e_new.get_Value().set_utitlity_inc_c(cost_inc);
                x_new.get_Value().set_utility_c(x_nearest.get_Value().get_utility_c() + cost_inc);
                x_new.add_ParentEdge(e_new);

                if (Draining(x_new, x_nearest, delta_compete))
                {
                    V_elite.AddPoint(x_new.get_Value().get_q(), x_new);
                    x_nearest.childNumberUp();


                    if (doGoalCheck == true && x_new.isDrained() == false)
                    {
                        if (System.GoalReached(x_new.get_Value().get_q(), q_goal, r_goal))
                        {
                            if (this.solution != null)
                            {
                                if (x_new.get_Value().get_utility_c() < this.solution.GetCost())
                                {
                                    lock (x_new)
                                    {
                                        this.solution = new Trajectory(x_new);
                                    }
                                }
                            }
                            else
                                this.solution = new Trajectory(x_new);
                        }
                    }
                }
            }
        }

        private double EnergyTotal()
        {
            throw new NotImplementedException();
        }

        public bool breakHere = false;

        private bool Draining(Node_ioRRT x_new, Node_ioRRT x_nearest, double radius)
        {
            List<object> localNeighbors = null;
            GetVertices_Eilte().NearestNeighbors_WithinRange_Exclusive(x_new.get_Value().get_q(), radius, ref localNeighbors);
            double cost_new = x_new.get_Value().get_utility_c();

            check_point_4++; //-----------------------------------

            for (int i = 0; i < localNeighbors.Count; ++i)
            {
                Node_ioRRT x_i = (Node_ioRRT)localNeighbors[i];
                double cost_i = x_i.get_Value().get_utility_c();
                if (cost_i <= cost_new && x_i != x_nearest)
                {
                    // x_new does not bring benefit, it is part of the graph. And it should be exterminated.
                    lock (x_new)
                    {
                        x_new.set_Parents(null);
                        x_new.MarkAsDrained();
                    }
                    return false;
                }
            }

            check_point_5++; //-----------------------------------

            for (int i = 0; i < localNeighbors.Count; ++i)
            {
                Node_ioRRT x_i = (Node_ioRRT)localNeighbors[i];
                if (x_i == x_nearest)
                    continue;
                GetVertices_Eilte().DelPoint(x_i.get_Value().get_q(), x_i);
                x_i.MarkAsDrained();

                #region Remove loose-end

                Node_ioRRT x_cur = x_i;
                while ( x_cur.IsLeafNode() &&
                        x_cur != x_nearest &&
                        x_cur.isDrained() )
                {
                    List<Edge> E_parent = x_cur.get_Parents();
                    if (E_parent == null) break;
                    Edge e = E_parent[0];
                    Node_ioRRT x_parent = (Node_ioRRT)e.get_entry();

                    DisconnectNode(x_cur);
                    x_parent.childNumberDown();

                    check_point_2++; //-------------------------------------

                    x_cur = x_parent;

                    if (x_cur == null)
                        break;
                }
                #endregion
            }

            return true;
        }

        private void DisconnectNode(Node_ioRRT x)
        {
            //V_elite.DelPoint(x.get_Value().get_q(), x);

            //List<Edge> E_parents = x.get_Parents();
            //if (E_parents == null) return;
            //Edge e_parent = E_parents[0];
            //e_parent.set_entry(null);
            //e_parent.set_exit(null);
            //e_parent.set_Value(null);
            //E_parents.Clear();
            //x.set_Value(null);
            lock (x)
            {
                x.set_Parents(null);
            }

        }

        private Node GetParentOfA(Node x_A)
        {
            List<Edge> E = x_A.get_Parents();
            if (E == null)
                return null;
            if (E.Count == 0)
                return null;
            if (E.Count != 1)
                throw new Exception("multiple parent situation occured.");
            return E[0].get_entry();
        }

        public override Node SpawnNode_Like(double[] q)
        {
            int DOF_state = this.System.Get_DOF_state();
            double[] q_new = new double[DOF_state];
            for (int i = 0; i < DOF_state; ++i)
                q_new[i] = q[i];
            Node_ioRRT x = Node_ioRRT.SpawnGraphicNode();
            x.set_Value(new NodeData_ioRRT(q_new, 0));
            return x;
        }
        public override Node SpawnNode_As(double[] q)
        {
            if (q.Length != this.System.Get_DOF_state())
                throw new Exception("q.length != DOF_state. in bioRRT.SpawnNode_As");
            Node_ioRRT x = Node_ioRRT.SpawnGraphicNode();
            x.set_Value(new NodeData_ioRRT(q, 0));
            return x;
        }
        public override Edge SpawnEdge()
        {
            Edge e = Edge.SpawnGraphicEdge();
            e.set_Value(new EdgeData_ioRRT());
            return e;
        }
        public override Edge SpawnEdge(Node entry, Node exit)
        {
            Edge e = Edge.SpawnGraphicEdge(entry, exit);
            e.set_Value(new EdgeData_ioRRT());
            return e;
        }
        public override string ToString()
        {
            throw new NotImplementedException();
        }
        public override void Test()
        {
            throw new NotImplementedException();
        }

        /// <summary>
        ///  test if q_test is in the goal region
        /// </summary>
        /// <param name="q_test"></param>
        /// <returns></returns>
        public override bool GoalReached(double[] q_test)
        {
            if (r_goal <= 0)
                throw new NotImplementedException("Forget to call SetGoal and SetGoalTolerance first?");
            double distance2 = this.System.Distance2_Topological(q_goal, q_test);

            double distance = Math.Sqrt(distance2);

            if (distance2 <= r_goal * r_goal)
                return true;
            else
                return false;
        }
        public double GetGoalTolerance()
        {
            return this.r_goal;
        }
        public void SetGoalTolerance(double radius)
        {
            this.r_goal = radius;
        }
        override public void SetGoal(double[] q_goal)
        {
            this.q_goal = q_goal;
            this.solution = null;
        }
        public double[] GetGoal()
        {
            return q_goal;
        }
        public void SetGoalCheck(bool checkSwitch)
        {
            doGoalCheck = checkSwitch;
        }
        public PointSetComponent GetVertices_Eilte()
        {
            return V_elite;
        }
        public PointSetComponent GetVertices_Elder()
        {
            return V_elder;
        }

        override public void Plot()
        {
            List<object> graph = V_elite.GetAllDataObjects();
            Visualizer.PlotVerticesWithCost(graph);
        }

        override public string MonitorPlannerState()
        {
            StringBuilder monitor_msg = new StringBuilder();
            monitor_msg
                .Append("check_point_0 = " + check_point_0)
                .Append("\ncheck_point_1 = " + check_point_1)
                .Append("\ncheck_point_2 = " + check_point_2)
                .Append("\ncheck_point_3 = " + check_point_3)
                .Append("\ncheck_point_4 = " + check_point_4)
                .Append("\ncheck_point_5 = " + check_point_5);
            return monitor_msg.ToString();
        }
    }
}
